//装甲板可视化
#include"Dector.hpp"

//显示帧率
void showFPS(cv::VideoCapture & Cap,cv::Mat & inputImage)
{
  double fps = Cap.get(cv::CAP_PROP_FPS);    //利用VideoCapture类的get()方法求帧率，并赋值给fps
  std::stringstream ss1;         //sstream对象ss1 用于构建一个包含帧率信息的字符串
  ss1 << "FPS: " << fps << " ";   
  std::string fpsText = ss1.str();   //含帧率信息的字符串
  cv::putText(inputImage, fpsText, cv::Point(10, 60), cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(0, 255, 0), 4);
}

//进行颜色分割，返回二值化图像
cv::Mat ColorFind(cv::Mat srcImage,                 
                  int iLowH,int iHighH,    // HSV的色调、饱和度、亮度的上下限
                  int iLowS,int iHighS,
                  int iLowV,int iHighV)
{
  cv::Mat imgBW;
  cv::Mat imgHSV;
  cv::cvtColor(srcImage,imgHSV,cv::COLOR_BGR2HSV);  //转化成HSV图像
  cv::inRange(imgHSV,cv::Scalar(iLowH,iLowS,iLowV),cv::Scalar(iHighH,iHighS,iHighV),imgBW);  //颜色筛选
  return imgBW;  //返回一个二值化图像
}

//红色分割，返回二值化图像
cv::Mat ColorFind_RGB::ColorFind_Red(cv::Mat srcImage) 
{
  cv::Mat des_red = ColorFind(srcImage,
                5,63,                       //调用ColorFind函数，红色灯条部分变白，其余变黑
                0,255,
                198,255);
  return des_red;  //返回一个二值化图像(红为白，其余为黑)
}

//蓝色分割，返回二值化图像
cv::Mat ColorFind_RGB::ColorFind_Blue(cv::Mat srcImage)
{
  cv::Mat des_blue = ColorFind(srcImage,
                    80, 138,                //调用ColorFind函数，蓝色灯条部分变白，其余变黑
                    0,255,           
                    198,255);
  return des_blue;  //返回一个二值化图像(蓝为白，其余为黑)
}

//利用HSV阈值判断是否为红色
bool ColorBool_RGB::ColorFindRedBool(cv::Mat srcImage)
{
  cv::Mat des1 = ColorFind(srcImage,          //调用ColorFind函数进行第一次红色区域查找
                175, 180,                   
                178,255,
                153,255);
  cv::Mat des2 = ColorFind(srcImage,          //调用ColorFind函数进行第二次红色区域查找
                0, 5,
                178,255,
                153,255); 
  cv::Mat des3 = des1 + des2;            //将两次查找得到的红色区域合并在一起，扩大覆盖范围 
  std::vector<cv::Mat> des3_;
  cv::split(des3,des3_);    //分离通道，因为下面的countNonZero函数要输入参数必须是单通道
  return(cv::countNonZero(des3_[0])==0);    //如果非零元素数量为0，说明图像中不存在符合指定红色范围的区域，函数返回true，否则返回false
}

//利用HSV阈值判断是否为蓝色
bool ColorBool_RGB::ColorFindBlueBool(cv::Mat srcImage)
{
  cv::Mat des2 = ColorFind(srcImage,          //调用ColorFind函数进行蓝色区域查找
                    105, 123,
                    153,255,
                    238,255);
  std::vector<cv::Mat> des2_;
  cv::split(des2,des2_);    //与红色部分同理
  return(cv::countNonZero(des2_[0])==0);      //与红色部分同理
}

//测量装甲板二维速度
void ArmorVelocity::measureVelocity(const cv::Point2f& currentVertex)
{  
  if (isFirstFrame) {     //检查是否是第一帧，如果是就执行代码块
    previousVertex = currentVertex;    
    currentVelocity = cv::Point2f(0, 0);   //归零
    isFirstFrame = false;   //确保初始化操作仅在第一帧时执行一次
    return;    //不继续执行下面代码块
  }
  // 计算时间间隔（单位：秒），通过帧率计算                                
  double deltaT = 1.0 / frameRate;   // △t = 1/fps
  // 计算位置变化
  cv::Point2f deltaPosition = currentVertex - previousVertex;   // (△x ,△y)
  // 计算分速度（像素/秒）
  cv::Point2f velocityPixel(deltaPosition.x / deltaT, deltaPosition.y / deltaT);  //x分量速度和y分量速度
  // 转换为米/秒
  float pixelToMeterRatio = 0.00057143;   //像素单位转换为米单位的比例系数（装甲板长度为140mm,视频中装甲板左右两边像素差为245，所以比例系数为0.14/245 ≈ 0.00057143）
  currentVelocity = cv::Point2f(velocityPixel.x * pixelToMeterRatio, velocityPixel.y * pixelToMeterRatio); 
  //计算二维速度（米/秒）
  TwoVelocity = std::sqrt(currentVelocity.x * currentVelocity.x + currentVelocity.y * currentVelocity.y); //  TwoVelocity = √x²+y²
  // 更新顶点信息，继续下一次测量
  previousVertex = currentVertex;
}

//在视频中显示速度
void ArmorVelocity::showVelocity(cv::Mat& inputImage) 
{
  std::stringstream ss2;      //sstream对象ss2 用于构建一个包含速度信息的字符串
  ss2 << "TwoVelocity(m/s): " << TwoVelocity << " ";   
  std::string velocityText = ss2.str();
  cv::putText(inputImage, velocityText, cv::Point(10, 110), cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(0, 255, 0), 4);
}

//更新获取的顶点信息以继续计算速度
void ArmorInfo::updateVertices(const cv::Point2f& newVertex) 
{
  vertex = newVertex;    //传递新的顶点信息
  velocity.measureVelocity(vertex);   //把新信息传递给measureVelocity方法
}

//识别红蓝
void DetectOperation::findRedOrBlue(cv::Mat & inputImage,cv::Mat & outputImage,std::string & color)
{  
   ColorBool_RGB Bool; //创建ColorBool_RGB类的对象Bool
   ColorFind_RGB find; //创建ColorFind_RGB类的对象find
   bool isRed = Bool.ColorFindBlueBool(inputImage);    //调用ColorBool_RGB类的ColorFindBlueBool函数，并赋值给isRed
   if(!isRed){          //如果他是蓝，执行蓝色二值化，返回outputImage和color
    outputImage = find.ColorFind_Blue(inputImage);
    color = "blue";}       
   else {               //否则执行红色二值化，返回outputImage和color
    outputImage = find.ColorFind_Red(inputImage);
    color = "red";}
}

//图像处理
void DetectOperation::imageProcess(cv::Mat & inputImage,cv::Mat & outputImage)
{   
    cv::Mat imgBlur;
    GaussianBlur(inputImage, imgBlur, cv::Size(5, 5), 0);   //高斯模糊
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE,cv::Size(3,3));  
    cv::dilate(imgBlur, outputImage, kernel,cv::Point(-1,-1),2);  //膨胀两次
}

//筛选轮廓
void DetectOperation::filterContours(std::vector<std::vector<cv::Point>> &contours, 
std::vector<LightDescriptor>& lightInfos) 
{   //遍历所有轮廓
    for (int i = 0; i < contours.size(); i++) {
        // 求轮廓面积
        double area = cv::contourArea(contours[i]);
        // 去除较小轮廓
        if (area < 550 || contours[i].size() <= 1)
            continue;
        // 用椭圆拟合区域得到外接矩形（特殊的处理方式：因为灯条是椭圆型的，所以用椭圆去拟合轮廓，再直接获取旋转外接矩形即可）
        cv::RotatedRect Light_Rec = cv::fitEllipse(contours[i]);
        // 长宽比限制（由于要考虑灯条的远近都被识别到，所以只需看比例）
        if (Light_Rec.size.width / Light_Rec.size.height < 0.06 ||
        Light_Rec.size.width / Light_Rec.size.height > 0.55)
            continue;
        // 扩大灯柱的面积（相当于对灯柱做膨胀操作）
        Light_Rec.size.height *= 1.1;
        Light_Rec.size.width *= 1.1;
        //把筛选完剩下的部分放到lightInfos
        lightInfos.push_back(LightDescriptor(Light_Rec)); 
    }
}

//装甲板可视化，读取装甲板位置信息
void DetectOperation::armorDetect(std::vector<LightDescriptor>& lightInfos,
cv::Mat & inputImage,std::string & color,std::vector<ArmorInfo>& armors,double frameRate)
{
  std::vector<ArmorInfo> currentArmors;//用于接收ArmorInfo对象
  //遍历所有灯条进行匹配（双重循环，在原先的i灯条的基础上去匹配剩下的灯条，即筛选出来）
  for (int i = 0; i < lightInfos.size(); i++) {       //外层循环遍历每一个灯条                 
    for (int j = i + 1; j < lightInfos.size(); j++) {     //内层循环从当前灯条的下一个开始遍历，形成灯条对
        LightDescriptor& leftLight = lightInfos[i];        //创建一个左灯条信息
        LightDescriptor& rightLight = lightInfos[j];       //创建一个右灯条信息
        float meanLen = (leftLight.length + rightLight.length) / 2;    //两个灯条的平均长度
        float dis = std::sqrt(std::pow(leftLight.center.x - rightLight.center.x, 2) + std::pow(leftLight.center.y - rightLight.center.y, 2));//两个灯条间的距离 dis=√△x²+△y²
        float yDiff = std::abs(leftLight.center.y - rightLight.center.y);  //y差
        float xDiff = std::abs(leftLight.center.x - rightLight.center.x);  //x差
        float yDiff_ratio = yDiff / meanLen;   //y差率
        float xDiff_ratio = xDiff / meanLen;   //x差率
        // 进一步筛选
        if (yDiff_ratio > 0.3 || xDiff_ratio > 2.6 ) 
            continue;
        // 绘制四边形、文本
        cv::Point center((leftLight.center.x + rightLight.center.x) / 2, (leftLight.center.y + rightLight.center.y) / 2);  //旋转矩形的中心
        cv::RotatedRect rect(center, cv::Size(dis, meanLen), 0);  //旋转矩形
        cv::Point2f vertices[4];   //创建四个顶点
        rect.points(vertices);     //得出旋转矩形的四个点，存放在vertices
        for (int k = 0; k < 4; k++) {   //画四次
            cv::line(inputImage, vertices[k], vertices[(k + 1) % 4], cv::Scalar(0, 225, 0), 3);  //按照旋转矩形结构来逐一画线
        }
        if (color == "blue") {       //如果识别出灯条颜色是蓝色，输出文本"color = blue"
            cv::putText(inputImage, "color = blue", vertices[1], cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(0, 255, 255), 4);
        } else {                     //否则输出文本"color = red"
            cv::putText(inputImage, "color = red", vertices[1], cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(0, 255, 255), 4);
        }
        bool foundMatch = false;  //标记ArmorInfo对象是否在armors容器,没有就为false
        //遍历armors容器的每一个ArmorInfo对象
        for (ArmorInfo& armor : armors) {
                armor.updateVertices(vertices[1]); //调用ArmorInfo类的updateVertices函数，将vertices[1](左上角)作为参数传递给该函数，用于更新当前ArmorInfo对象的顶点信息
                currentArmors.push_back(armor);    //将更新后的ArmorInfo对象添加到currentArmors容器的末尾
                foundMatch = true;    //true代表有对象
        }
        if (!foundMatch) {                  //如果没有对象就执行代码块，避免currentArmors容器没有对象
            ArmorInfo armor(frameRate);        //创建一个新的ArmorInfo对象armor,并初始化  
            armor.updateVertices(vertices[1]);  
            currentArmors.push_back(armor);  
        }
    }
  }
  armors = currentArmors;   // 更新装甲板信息
}
